//
// Created by lijia on 2021/1/12.
//

#include "servo.h"

static uint8_t armed = 0;

void servo_init() {
	armed = 0;
}

void servo_arm() {
	armed = 1;
}

void servo_zeroing() {
	if (armed) {
		servo_set_yaw(0.f);
		servo_set_pitch(0.f);
	}
}

void servo_fulling() {
	if (armed) {
		servo_set_yaw(1.f);
		servo_set_pitch(1.f);
	}
}

void servo_disarm() {
	armed = 0;
}

/**
 * Set pitch (CH1).
 *
 * 0.f - Level
 * 1.f - 180deg
 *
 * @param x New pitch
 */
void servo_set_pitch(float x) {
	if (armed) {
		uint16_t out = PWM_ARM_ZERO + PITCH_BIAS;
		out += (uint16_t) ((PWM_ARM_FULL - PWM_ARM_ZERO) * x);
		pwm_set_ch1_counter(out);
	}
}

/**
 * Set yaw (CH2).
 *
 * 0.f - Pan right
 * 1.f - Pan left
 *
 * @param x New yaw
 */
void servo_set_yaw(float x) {
	if (armed) {
		uint16_t out = PWM_ARM_ZERO + YAW_BIAS;
		out += (uint16_t) ((PWM_ARM_FULL - PWM_ARM_ZERO) * x);
		pwm_set_ch2_counter(out);
	}
}

void servo_set_pitch_deg(uint16_t x) {
	if (x < 0) {
		x = 0;
	}
	if (x > 1800) {
		x = 1800;
	}
	servo_set_pitch(((float) x) / 1800.f);
}

void servo_set_yaw_deg(uint16_t x) {
	if (x > 1800) {
		servo_set_yaw(((float) (x - 1800)) / 1800.f);
	} else {
		servo_set_yaw(((float) x) / 1800.f);
	}
}
